//% color="#0070d6" iconWidth=50 iconHeight=40
namespace df_mecanum{
    //% block="基础功能" blockType="tag"
    export function tagtest() {}


    //% block="控制底盘电机以[SPEED]转/分的速率[DIR_FIX]平移" blockType="command" 
    //% SPEED.shadow="range" SPEED.defl=100  SPEED.params.min=0 SPEED.params.max=205
    //% DIR_FIX.shadow="dropdown" DIR_FIX.options="DIR_FIX"
    export function DF_mecanum_setrpmspeed(parameter: any, block: any) {
        let speed = parameter.SPEED.code;
        let dir_fix = parameter.DIR_FIX.code;

        Generator.addImport(`from mecanum_drive_modbus import MecanumWheelController`);
        Generator.addInitHeader('DF_mecanum_init',`MecanumWheel = MecanumWheelController()\nwhile MecanumWheel.connect() == False:\n    print("wait for connect")\n`);
        Generator.addCode(`MecanumWheel.set_all_staus(${dir_fix},${speed})`);
   
    }

    //% block="控制底盘以[SPEED]%的速率[DEGREES]度方向平移" blockType="command" 
    //% SPEED.shadow="range" SPEED.params.min=0 SPEED.params.max=100 SPEED.defl=50 
    //% DEGREES.shadow="number" DEGREES.defl="60"
    export function DF_mecanum_settranslation(parameter: any, block: any) {
        let speed = parameter.SPEED.code;
        let degrees = parameter.DEGREES.code;
        Generator.addImport(`from mecanum_drive_modbus import MecanumWheelController`);
        Generator.addInitHeader('DF_mecanum_init',`MecanumWheel = MecanumWheelController()\nwhile MecanumWheel.connect() == False:\n    print("wait for connect")\n`);
        Generator.addCode(`MecanumWheel.run(${degrees},${speed},0)`);
    }
    //% block="控制底盘以[SPEED]%的速率[TROTA]自转" blockType="command" 
    //% SPEED.shadow="range" SPEED.params.min=0 SPEED.params.max=100 SPEED.defl=50 
    //% TROTA.shadow="dropdown" TROTA.options="TROTA"
    export function DF_mecanum_setrotation(parameter: any, block: any) {
        let speed = parameter.SPEED.code;
        let trota = parameter.TROTA.code;
        Generator.addImport(`from mecanum_drive_modbus import MecanumWheelController`);
        Generator.addInitHeader('DF_mecanum_init',`MecanumWheel = MecanumWheelController()\nwhile MecanumWheel.connect() == False:\n    print("wait for connect")\n`);
        if (trota == 0){
            Generator.addCode(`MecanumWheel.run(0,0,${speed})`);
        }else if (trota == 1){  
            Generator.addCode(`MecanumWheel.run(0,0,-${speed})`);
        }
    }
    //% block="控制底盘以[SPEED]%的速率[TROTA]做圆周运动" blockType="command" 
    //% SPEED.shadow="range" SPEED.params.min=0 SPEED.params.max=100 SPEED.defl=50 
    //% TROTA.shadow="dropdown" TROTA.options="TROTA"
    export function DF_mecanum_setrotation1(parameter: any, block: any) {
        let speed = parameter.SPEED.code;
        let trota = parameter.TROTA.code;
        Generator.addImport(`from mecanum_drive_modbus import MecanumWheelController`);
        Generator.addInitHeader('DF_mecanum_init',`MecanumWheel = MecanumWheelController()\nwhile MecanumWheel.connect() == False:\n    print("wait for connect")\n`);
        if (trota == 0){
            Generator.addCode(`MecanumWheel.run(0,${speed},-90)`);
        }else if (trota == 1){  
            Generator.addCode(`MecanumWheel.run(0,${speed},90)`);
        }
    }
    //% block="控制底盘电机[MOTOR]以[SPEED]转/分的速率[DIR]" blockType="command" 
    //% MOTOR.shadow="dropdown" MOTOR.options="MOTOR"
    //% SPEED.shadow="range" SPEED.defl=100 SPEED.params.min=0 SPEED.params.max=205
    //% DIR.shadow="dropdown" DIR.options="DIR"
    export function DF_mecanum_setmotorspeed(parameter: any, block: any) {
        let motor = parameter.MOTOR.code;
        let speed = parameter.SPEED.code;
        let dir = parameter.DIR.code;
        Generator.addImport(`from mecanum_drive_modbus import MecanumWheelController`);
        Generator.addInitHeader('DF_mecanum_init',`MecanumWheel = MecanumWheelController()\nwhile MecanumWheel.connect() == False:\n    print("wait for connect")\n`);
        Generator.addCode(`MecanumWheel.set_speed(${dir},${motor},${speed})`);
    }
    //% block="控制底盘[MOTOR]停止" blockType="command" 
    //% MOTOR.shadow="dropdown" MOTOR.options="MOTOR"
    export function DF_mecanum_setstop(parameter: any, block: any) {

        let motor = parameter.MOTOR.code;
        Generator.addImport(`from mecanum_drive_modbus import MecanumWheelController`);
        Generator.addInitHeader('DF_mecanum_init',`MecanumWheel = MecanumWheelController()\nwhile MecanumWheel.connect() == False:\n    print("wait for connect")\n`);
        Generator.addCode(`MecanumWheel.stop(${motor})`);
       
    } 

    //% block="控制[SERVO]舵机转到[DEGREES]度" blockType="command" 
    //% SERVO.shadow="dropdown" SERVO.options="SERVO"
    //% DEGREES.shadow="angle" DEGREES.params.edge=1
    export function DF_mecanum_setServo(parameter: any, block: any) {
        let servo = parameter.SERVO.code;
        let degrees = parameter.DEGREES.code;
        Generator.addImport(`from mecanum_drive_modbus import MecanumWheelController`);
        Generator.addInitHeader('DF_mecanum_init',`MecanumWheel = MecanumWheelController()\nwhile MecanumWheel.connect() == False:\n    print("wait for connect")\n`);
        Generator.addCode(`MecanumWheel.set_degree(${servo},${degrees})`);
       
    } 
    //% block="拓展功能" blockType="tag"
    export function tagtest1() {}
    //% block="设置底盘为[MODE]模式  " blockType="command" 
    //% MODE.shadow="dropdown" MODE.options="MODE"
    export function DF_mecanum_setmode(parameter: any, block: any) {
        let mode = parameter.MODE.code;
        Generator.addImport(`from mecanum_drive_modbus import MecanumWheelController`);
        Generator.addInitHeader('DF_mecanum_init',`MecanumWheel = MecanumWheelController()\nwhile MecanumWheel.connect() == False:\n    print("wait for connect")\n`);
        Generator.addCode(`MecanumWheel.set_mode(${mode})`);
      }   
    //% block="PID[PID]" blockType="command" 
    //% PID.shadow="dropdown" PID.options="PID"
      export function DF_mecanum_setpidstaus(parameter: any, block: any) {
        let pid = parameter.PID.code;
        Generator.addImport(`from mecanum_drive_modbus import MecanumWheelController`);
        Generator.addInitHeader('DF_mecanum_init',`MecanumWheel = MecanumWheelController()\nwhile MecanumWheel.connect() == False:\n    print("wait for connect")\n`);
        Generator.addCode(`MecanumWheel.enable_pid(${pid})`);
      }
    //% block="设置Kp的值为[KP]Ki的值为[KI]Kd的值为[KD]" blockType="command" 
    //% KP.shadow="number" KP.defl=40
    //% KI.shadow="number" KI.defl=2
    //% KD.shadow="number" KD.defl=2
    export function DF_mecanum_setpid(parameter: any, block: any) {
        let kp = parameter.KP.code;
        let ki = parameter.KI.code;
        let kd = parameter.KD.code;
        Generator.addImport(`from mecanum_drive_modbus import MecanumWheelController`);
        Generator.addInitHeader('DF_mecanum_init',`MecanumWheel = MecanumWheelController()\nwhile MecanumWheel.connect() == False:\n    print("wait for connect")\n`);
        Generator.addCode(`MecanumWheel.set_pid_values(${kp},${ki},${kd})`);
    }
    //% block="控制底盘电机[MOTOR]以[SPEED]%PWM[DIR]" blockType="command" 
    //% MOTOR.shadow="dropdown" MOTOR.options="MOTOR"
    //% SPEED.shadow="range" SPEED.defl=50 SPEED.params.min=0 SPEED.params.max=100
    //% DIR.shadow="dropdown" DIR.options="DIR"
    export function DF_mecanum_setmotorpwm(parameter: any, block: any) {
        let motor = parameter.MOTOR.code;
        let speed = parameter.SPEED.code;
        let dir = parameter.DIR.code;
        Generator.addImport(`from mecanum_drive_modbus import MecanumWheelController`);
        Generator.addInitHeader('DF_mecanum_init',`MecanumWheel = MecanumWheelController()\nwhile MecanumWheel.connect() == False:\n    print("wait for connect")\n`);
        Generator.addCode(`MecanumWheel.set_pwm(${dir},${motor},${speed})`);
    } 
    //% block="读取编码器[MOTOR]的值" blockType="reporter" 
    //% MOTOR.shadow="dropdown" MOTOR.options="MOTOR"
    export function DF_mecanum_readmotorpwm(parameter: any, block: any) {
        let motor = parameter.MOTOR.code;
        Generator.addImport(`from mecanum_drive_modbus import MecanumWheelController`);
        Generator.addInitHeader('DF_mecanum_init',`MecanumWheel = MecanumWheelController()\nwhile MecanumWheel.connect() == False:\n    print("wait for connect")\n`);
        Generator.addCode(`MecanumWheel.read_real_speed(${motor})`);
    }   
}